#include "mylib.h"

#if HAVE_FIFO_H

#define ilog2(n)				\
( 							\
	(n) & (1ULL << 31) ? 31 :	\
	(n) & (1ULL << 30) ? 30 :	\
	(n) & (1ULL << 29) ? 29 :	\
	(n) & (1ULL << 28) ? 28 :	\
	(n) & (1ULL << 27) ? 27 :	\
	(n) & (1ULL << 26) ? 26 :	\
	(n) & (1ULL << 25) ? 25 :	\
	(n) & (1ULL << 24) ? 24 :	\
	(n) & (1ULL << 23) ? 23 :	\
	(n) & (1ULL << 22) ? 22 :	\
	(n) & (1ULL << 21) ? 21 :	\
	(n) & (1ULL << 20) ? 20 :	\
	(n) & (1ULL << 19) ? 19 :	\
	(n) & (1ULL << 18) ? 18 :	\
	(n) & (1ULL << 17) ? 17 :	\
	(n) & (1ULL << 16) ? 16 :	\
	(n) & (1ULL << 15) ? 15 :	\
	(n) & (1ULL << 14) ? 14 :	\
	(n) & (1ULL << 13) ? 13 :	\
	(n) & (1ULL << 12) ? 12 :	\
	(n) & (1ULL << 11) ? 11 :	\
	(n) & (1ULL << 10) ? 10 :	\
	(n) & (1ULL <<  9) ?  9 :	\
	(n) & (1ULL <<  8) ?  8 :	\
	(n) & (1ULL <<  7) ?  7 :	\
	(n) & (1ULL <<  6) ?  6 :	\
	(n) & (1ULL <<  5) ?  5 :	\
	(n) & (1ULL <<  4) ?  4 :	\
	(n) & (1ULL <<  3) ?  3 :	\
	(n) & (1ULL <<  2) ?  2 :	\
	(n) & (1ULL <<  1) ?  1 :	\
	 0 \
)

#define roundup_pow_of_two(n)	(1UL << (ilog2((n) - 1) + 1))

#define rounddown_pow_of_two(n)	(1UL << ilog2(n))

#define is_power_of_2(n) ((n) != 0 && (((n) & ((n) - 1)) == 0))

static inline unsigned int fifo_len(struct fifo *self)
{
	register unsigned int out;
	out = self->out;
	return self->in - out;
}

static inline unsigned int fifo_size(struct fifo *self)
{
	return self->size;
}

static inline void fifo_reset(struct fifo *self)
{
	self->in = self->out = 0;
}

static inline int fifo_is_empty(struct fifo *self)
{
	return self->in == self->out;
}

static inline int fifo_is_full(struct fifo *self)
{
	return fifo_len(self) == fifo_size(self);
}

static inline unsigned int fifo_avail(struct fifo *self)
{
	return fifo_size(self) - fifo_len(self);
}

struct fifo *fifo_init(unsigned int size)
{
	struct fifo *pst;
	pst = Malloc(sizeof(struct fifo));
	if (!is_power_of_2(size)) 
	{
		size = roundup_pow_of_two(size);
	}
	pst->size = size;
	pst->in = pst->out = 0;
	pst->buffer = Malloc(size);
	return pst;
}

void fifo_destroy(struct fifo *self)
{
	free(self->buffer);
	free(self);
}

unsigned int fifo_write(struct fifo *self, const void *from, unsigned int len)
{
	unsigned int off,l;
	len = min(fifo_avail(self), len);
	off = self->in & (self->size - 1);
	l = min(len, self->size - off);
	memcpy(self->buffer+off, from, l);
	memcpy(self->buffer, from + l, len - l);
	self->in += len;
	return len;
}



unsigned int fifo_read(struct fifo *self, void *to, unsigned int len)
{
	unsigned int off,l;
	len = min(fifo_len(self), len);
	off = self->out & (self->size - 1);
	l = min(len, self->size - off);
	memcpy(to, self->buffer + off, l);
	memcpy(to + l, self->buffer, len - l);
	self->out += len;
	return len;
}
#endif
